#!/bin/bash

source /opt/ros/humble/setup.bash

export SLAM_ROOT=/opt/carto_ros2

LOCAL_IP=192.168.1.117
export ROS_MASTER_URI=http://$LOCAL_IP:11311
export ROS_HOSTNAME=$LOCAL_IP
export ROS_IP=$LOCAL_IP

check_process() {
	pgrep -f "$1" > /dev/null 2>&1
	return $?
}

kill_process(){
    PID=$(pgrep -f "$1")
    if [ $? -eq 0 ]; then
        kill -SIGKILL $PID > /dev/null 2>&1
    fi
}

# ROS Core

wait_roscore(){
    while true; do
        rosnode list > /dev/null 2>&1
        if [ $? -eq 0 ]; then
            break
        fi
        sleep 1
    done
}

before_roscore(){
    kill_process "param_space_node"
    kill_process "robot_state_publisher"
}

after_roscore(){
    wait_roscore
    /opt/carto/bin/param_space/param_space_node > /opt/carto/log/param_space_node.log 2>&1 &
    rosparam load "/opt/carto/assets/robot.urdf" /robot_description
    rosrun robot_state_publisher robot_state_publisher > /opt/carto/log/robot_state_publisher.log 2>&1 &
}

check_roscore(){
    check_process "bin/roscore"
    if [ $? -ne 0 ]; then
        echo "roscore is not running, start it";
        before_roscore
        roscore > /opt/carto/log/roscore.log 2>&1 &
        after_roscore
    fi
}

# Cartographer

before_carto(){
    kill_process "cartographer_node"
    kill_process "cartographer_occupancy_grid_node"
    kill_process "tcp_server_node"
    kill_process "pub_landmark"
}

after_carto(){
    rosparam load "/opt/carto/config/landmark.yaml"
    /opt/carto/bin/cartographer/cartographer_node > /opt/carto/log/cartographer_node.log 2>&1 &
    /opt/carto/bin/cartographer/cartographer_occupancy_grid_node > /opt/carto/log/cartographer_occupancy_grid_node.log 2>&1 &
    /opt/carto/bin/landmark/tcp_server_node > /opt/carto/log/tcp_server_node.log 2>&1 &
    /opt/carto/bin/landmark/pub_landmark > /opt/carto/log/pub_landmark.log 2>&1 &
}

check_carto(){
    check_process "cartographer_node"
    if [ $? -ne 0 ]; then
        echo "cartographer_node is not running, start it";
        before_carto
        after_carto
    fi
}

while true;do
    check_roscore
    check_carto
    sleep 1
done
